#include "gp_openlid_ctl.h"
#include "sock_msg_ctl.h"
#include "gp_layout_map.h"
#include "stdio.h"
#include "string.h"
#include "../lua/action_msg.h"
#include "resource_manage.h"
#include <unistd.h>

void openlidActFunc(void * paramIn,void * actResultOut)
{
    if(!paramIn || !actResultOut)
    {
        return;
    }
    printf("enter in openlidActFunc\n");
    act_msg_node * pstActMsg = (act_msg_node *)paramIn;
    int actSuccess = 0;
    
    if(pstActMsg->msgHead.actType == ACT_SINGLE_MOVE)
    {
        if(pstActMsg->msgHead.paramType == PARAM_AXIS_MOVE_STEPS)
        {
            int component = -1;
            int steps = 0;
            char * pData = (char*)pstActMsg->paramData;

            memcpy(&component,pData,sizeof(int));
            pData += sizeof(int);

            memcpy(&steps,pData,sizeof(int));
            actSuccess = openlid_single_move_steps(&steps,component);

        }
        else if(pstActMsg->msgHead.paramType == PARAM_AXIS_MOVE_POS)
        {
            int component = -1;
            location_layout locLayout;
            memset(&locLayout,0,sizeof(location_layout));
            char * pData = (char*)pstActMsg->paramData;

            memcpy(&component,pData,sizeof(int));
            pData += sizeof(int);

            memcpy(&locLayout,pData,sizeof(location_layout));
            actSuccess = openlid_single_move_pos(&locLayout,component);
        }
        
    }
    else if(pstActMsg->msgHead.actType == ACT_SINGLE_INIT)
    {
        int component = -1;
        memcpy(&component,(char*)pstActMsg->paramData,sizeof(int));
        printf("openlidActFunc,single init,componet:%d\n",component);
        actSuccess = openlid_single_init(NULL,component);
    }
    else if(ACT_CHAIN_ACTION == pstActMsg->msgHead.actType)
    {
        if(pstActMsg->msgHead.paramType == PARAM_OPENLID_CTL)
        {
            //int component = -1;
            //memcpy(&component,(char*)pstActMsg->paramData,sizeof(int));
            

            char * pData = (char*)pstActMsg->paramData;
            //pData += sizeof(int);

            int isOpen = 0;

            memcpy(&isOpen,pData,sizeof(int));

            if(isOpen)
            {//open lid of sample bottle
               printf("openlidActFunc,open lid sequence\n");
               actSuccess = openlid_open(); 
            }
            else
            {//close lid of sample bottle
               printf("openlidActFunc,close lid sequence\n");
               actSuccess = openlid_close();
            }
            
        }
        
    }
    else if(ACT_MODULE_INIT == pstActMsg->msgHead.actType)
    {
        actSuccess = openlid_module_init(NULL);
    }

    msgAckSock * pstResult = (msgAckSock *)actResultOut;

    if(!actSuccess)
    {
        pstResult->status = 1;
    }
    else
    {
        pstResult->status = 0;
    }

    
}

int convertToOpenlidSid(int componentIn)
{
    int sid = -1;
    switch (componentIn)
    {
        case MT_AXIS_Z:
        {
            sid = SID_OPENLID_Z;
        }
        break;
        case MT_AXIS_X:
        case MT_AXIS_Y:
        {
            sid = SID_OPENLID_X;
        }
        break;
        case MT_JAW:
        {
            sid = SID_OPENLID_JAW;
        }
        break;
        default:
        break;
    }
    return sid; 
}

int openlid_single_init(void * paramIn,int component)
{
    int sid = convertToOpenlidSid(component);
    int actSuccess = 0;
    if(sid==-1)
    {
        return actSuccess;
    }

    move_param moveParam;

    moveParam.sid = sid;
    moveParam.pos = 0;

    moveParam.dec_speed = 80000;
    moveParam.inc_speed = 80000;
    moveParam.speed = 20000;        

    
    actSuccess = uimInitAct_multi(&moveParam, 1, 20000);

    return actSuccess;
}


int openlid_module_init(void * paramIn)
{
    move_param moveParam[3];
    moveParam[0].sid = SID_OPENLID_JAW;
    moveParam[0].pos = 0;
    moveParam[0].dec_speed = 8000;
    moveParam[0].inc_speed = 8000;
    moveParam[0].speed = 4500;

    int actSuccess = uimInitAct_multi(moveParam, 1, 20000);

    if(actSuccess != 1)
    {
        printf("openlid jaw reset failed,actRet:%d\n",actSuccess);
        return actSuccess;
    }

    moveParam[0].sid = SID_OPENLID_Z;
    moveParam[0].pos = 0;
    moveParam[0].dec_speed = 80000;
    moveParam[0].inc_speed = 80000;
    moveParam[0].speed = 20000;
    moveParam[1].sid = SID_OPENLID_X;
    moveParam[1].pos = 0;
    moveParam[1].dec_speed = 32000;
    moveParam[1].inc_speed = 32000;
    moveParam[1].speed = 4000;

    actSuccess = uimInitAct_multi(moveParam, 2, 20000);

    if(actSuccess != 1)
    {
        printf("openlid x and z,actRet:%d\n",actSuccess);
        
    }
    return actSuccess; 
}


int openlid_single_move_steps(void * paramIn,int component)
{
    int sid = convertToOpenlidSid(component);
    int actSuccess = 0;
    if(sid==-1)
    {
        return actSuccess;
    }

    move_param moveParam;

    int * pSteps = (int *)paramIn; 

    moveParam.sid = sid;
    moveParam.pos = * pSteps;

    moveParam.dec_speed = 80000;
    moveParam.inc_speed = 80000;
    moveParam.speed = 20000;        

    
    actSuccess = uimMoveAct_multi(&moveParam, 1, 20000);

    return actSuccess;   
}

int openlid_single_move_pos(void * paramIn,int component)
{
    int actSuccess = 0;    
    int sid = -1;
    location_layout * pstLocLayoutIn = (location_layout *)paramIn;
    int pos = 0;
    sid = convertToOpenlidSid(component);

    if(sid==-1)
    {
        actSuccess = 0;
        printf("in openlid_single_move_pos,param not valid:component:%d\n",component);
        return actSuccess;
    }

    //locs_seal stOpenlidLoc;
    CGpLayout * pstLayout = CGpLayout::get_instance();
    locs_seal * pstOpenlidPos = NULL;//pstLayout->getOpenlidPos();
    
    move_param moveParamArr[2];
    memset(moveParamArr,0,sizeof(move_param)*2);
    
    if(pstLayout == NULL)
    {
        printf("in openlid_single_move_pos, get layout handle failed\n");
        return actSuccess;
    }

    pstOpenlidPos = pstLayout->getOpenlidPos();

    if(pstOpenlidPos==NULL)
    {
        printf("in openlid_single_move_pos, get openlid locs failed\n");
        return actSuccess;
    }

    int posToMove = -1;

    if(sid == SID_OPENLID_X)
    {
        if(pstLocLayoutIn->location_type == OPENLID_X_OPEN)
        {
            posToMove = pstOpenlidPos->x_work;
        }
        else if(pstLocLayoutIn->location_type == OPENLID_X_SUCK)
        {
            posToMove = pstOpenlidPos->x_wait;
        }
    }
    else if(sid == SID_OPENLID_Z)
    {
        if(pstLocLayoutIn->location_type == OPENLID_Z_OPEN)
        {
            posToMove = pstOpenlidPos->z_work;
        }
        else if(pstLocLayoutIn->location_type == OPENLID_Z_WAIT)
        {
            posToMove = pstOpenlidPos->z_wait;
        }        
    }
    else if(SID_OPENLID_JAW == sid)
    {
        if(pstLocLayoutIn->location_type == OPENLID_Z_OPEN)
        {
            posToMove = 0;
        }
        else if(pstLocLayoutIn->location_type == OPENLID_Z_WAIT)
        {
            posToMove = POS_JAW_OPENLID_CLOSE;
        }
    }

    if(posToMove < 0)
    {
        printf("in openlid_single_move_pos,_calc steps failed,locType:%d\n",pstLocLayoutIn->location_type);
        return actSuccess;
    }

    move_param moveParam;

    moveParam.sid = sid;
    moveParam.pos = posToMove;
    moveParam.dec_speed = 40000;
    moveParam.inc_speed = 40000;
    moveParam.speed = 10000;
    actSuccess = uimMoveAct_multi(&moveParam, 1, 20000);

    if(actSuccess != 1)
    {
        printf("in openlid_single_move_pos, move failed, actRet:%d\n",actSuccess);
        //return actSuccess;
    }
    return actSuccess;
}


int openlid_open(void)
{
    CGpLayout * pstLayout = CGpLayout::get_instance();
    locs_seal * pstOpenlidPos = NULL;//pstLayout->getOpenlidPos();
    move_param moveParamArr[2];
    memset(moveParamArr,0,sizeof(move_param)*2);
    int actSuccess = 0;

    if(pstLayout==NULL)
    {
        return actSuccess;
    }

    pstOpenlidPos = pstLayout->getOpenlidPos();
    if(pstOpenlidPos==NULL)
    {
       return actSuccess; 
    }

    printf("openlid,x_work:%d,x_wait:%d,z_work:%d,z_wait:%d\n",
    pstOpenlidPos->x_work,pstOpenlidPos->x_wait,
    pstOpenlidPos->z_work,pstOpenlidPos->z_wait);
    // //1, x go to work pos, and jaw open
    moveParamArr[0].pos = pstOpenlidPos->x_work;
    moveParamArr[0].speed = 32000;
    moveParamArr[0].inc_speed = 100000;    
    moveParamArr[0].dec_speed = 100000;
    moveParamArr[0].sid = SID_OPENLID_X;

    moveParamArr[1].pos = POS_JAW_OPENLID_OPEN;
    moveParamArr[1].speed = 4000;
    moveParamArr[1].inc_speed = 8000;    
    moveParamArr[1].dec_speed = 8000;
    moveParamArr[1].sid = SID_OPENLID_JAW;

    actSuccess = uimMoveAct_multi(moveParamArr,2,20000);

    if(actSuccess!=1)
    {
        printf("openlid_open, x goto work pos failed\n");
        return actSuccess;
    }

    //3, z go to work pos
    moveParamArr[0].pos = pstOpenlidPos->z_work;
    moveParamArr[0].speed = 20000;
    moveParamArr[0].inc_speed = 80000;    
    moveParamArr[0].dec_speed = 80000;
    moveParamArr[0].sid = SID_OPENLID_Z;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_open, z goto work pos failed\n");
        return actSuccess;
    }


    //4, jaw close
    moveParamArr[0].pos = POS_JAW_OPENLID_CLOSE;
    moveParamArr[0].speed = 20000;
    moveParamArr[0].inc_speed = 80000;    
    moveParamArr[0].dec_speed = 80000;
    moveParamArr[0].sid = SID_OPENLID_JAW;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_open, jaw close failed\n");
        return actSuccess;
    }

    //5, z  go up
    moveParamArr[0].pos = pstOpenlidPos->z_wait;
    moveParamArr[0].speed = 20000;
    moveParamArr[0].inc_speed = 80000;    
    moveParamArr[0].dec_speed = 80000;
    moveParamArr[0].sid = SID_OPENLID_Z;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_open, z go up failed\n");
        return actSuccess;
    }

    //6, x go to wait
    moveParamArr[0].pos = pstOpenlidPos->x_wait;
    moveParamArr[0].speed = 32000;
    moveParamArr[0].inc_speed = 160000;    
    moveParamArr[0].dec_speed = 160000;
    moveParamArr[0].sid = SID_OPENLID_X;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_open, x goto wait failed\n");
    }

    return actSuccess;
}

int openlid_close(void)
{
    CGpLayout * pstLayout = CGpLayout::get_instance();
    locs_seal * pstOpenlidPos = NULL;//pstLayout->getOpenlidPos();
    move_param moveParamArr[2];
    memset(moveParamArr,0,sizeof(move_param)*2);
    int actSuccess = 0;

    if(pstLayout==NULL)
    {
        return actSuccess;
    }

    pstOpenlidPos = pstLayout->getOpenlidPos();
    if(pstOpenlidPos==NULL)
    {
       return actSuccess; 
    }
    //1, x go to work pos
    moveParamArr[0].pos = pstOpenlidPos->x_work;
    moveParamArr[0].speed = 32000;
    moveParamArr[0].inc_speed = 160000;    
    moveParamArr[0].dec_speed = 160000;
    moveParamArr[0].sid = SID_OPENLID_X;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_close, x goto work pos failed\n");
        return actSuccess;
    }

    //3, z go to work pos
    moveParamArr[0].pos = pstOpenlidPos->z_work + 1000;
    moveParamArr[0].speed = 32000;
    moveParamArr[0].inc_speed = 160000;    
    moveParamArr[0].dec_speed = 160000;
    moveParamArr[0].sid = SID_OPENLID_Z;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_close, z goto work pos failed\n");
        return actSuccess;
    }


    //4, jaw open
    moveParamArr[0].pos = POS_JAW_OPENLID_OPEN;
    moveParamArr[0].speed = 32000;
    moveParamArr[0].inc_speed = 160000;    
    moveParamArr[0].dec_speed = 160000;
    moveParamArr[0].sid = SID_OPENLID_JAW;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_close, jaw open failed\n");
        return actSuccess;
    }

    //5, z go up
    moveParamArr[0].pos = pstOpenlidPos->z_wait;
    moveParamArr[0].speed = 20000;
    moveParamArr[0].inc_speed = 80000;    
    moveParamArr[0].dec_speed = 80000;
    moveParamArr[0].sid = SID_OPENLID_Z;

    actSuccess = uimMoveAct_multi(moveParamArr,1,20000);

    if(actSuccess!=1)
    {
        printf("openlid_close, z go up failed\n");
        return actSuccess;
    }

    //6, x go to wait, and jaw ret back
    moveParamArr[0].pos = pstOpenlidPos->x_wait;
    moveParamArr[0].speed = 32000;
    moveParamArr[0].inc_speed = 160000;    
    moveParamArr[0].dec_speed = 160000;
    moveParamArr[0].sid = SID_OPENLID_X;
    
    moveParamArr[1].pos = 0;
    moveParamArr[1].speed = 20000;
    moveParamArr[1].inc_speed = 80000;    
    moveParamArr[1].dec_speed = 80000;
    moveParamArr[1].sid = SID_OPENLID_JAW;

    actSuccess = uimMoveAct_multi(moveParamArr,2,20000);

    if(actSuccess!=1)
    {
        printf("openlid_close, x goto wait and jaw ret back failed\n");
    }
    return actSuccess;
}